COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE
C00005 ENDMK
C⊗;
REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE;

EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY TNS,ANGLES);
	! fills TNS with transpose(<arm trans>), ANGLES with
	  joint angles (ANGLES[7]=hand) in degrees & inches.
	  Returns 0 if all ok, otherwise returns ERROR CODE;


PRESET_WITH
	"ELF IS BUSY, SOMEONE IS USING IT",
	"ELF STATUS WORD INDICATES ERROR CONDITION",
	"UNABLE TO ZERO MASTER NUMBER IN ELF",
	"ELF NOT TRANSFERRING ARM POSITION",
	"BAD READ FROM ELF DURING POSITION DATA TRANSFER",
	"IOPDLOV WHEN TRYING TO SAVE CHANNEL 1 FOR ELF, TELL MSM ABOUT IT";
INTERNAL STRING ARRAY ARMERR[1:6];

INTERNAL REAL ARRAY BANGLE,YANGLE[1:7];

INTERNAL PROCEDURE READ_ARM(INTEGER ARM);
	BEGIN
	INTEGER I,J;
	OWN REAL ARRAY BESTNS[1:4,1:3], A[1:5,1:4],BESANGLES[1:7];
	IF ARM=BLUE
	THEN BEGIN
		ARMFALSE←TLKEF5(BESTNS,BESANGLES);
		IF ARMFALSE=0 THEN 
		BEGIN
			ARRCLR(A);
			FOR I← 1 STEP 1 UNTIL 3
				DO FOR J←1 STEP 1 UNTIL 4
					DO A[I,J]←BESTNS[J,I];
			A[4,4]←1.0;
			SETABS(F_BARM,A);
			FOR I←1 STEP 1 UNTIL 7 DO BANGLE[I]←BESANGLES[I];
			SCALAR:VALUE[S_BHAND] ←BESANGLES[7];	! RETURNS BLUE HAND OPENING;
		END
		ELSE 
		IF ¬$GTEXPR THEN
		ABORT1(ARMERR[ARMFALSE],NULL);
	     END
	ELSE ABORT1("CANT READ YELLOW ARM YET",NULL);

	END;

INTERNAL PROCEDURE READ_BLUE;
	READ_ARM(BLUE);